#ifndef ASTAR_HPP
#define ASTAR_HPP

#include <iostream>
#include "robot_one_navigation/A_Star/node.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"

namespace astar_planner
{
    class Astar
    {
    private:
        nav2_costmap_2d::Costmap2D *costmap_;
        int xs_; // 地图总列数（地图横向格子数）
        int ys_; // 地图总行数（地图纵向格子数）
        int ns_; // 地图所有栅格数
        bool allow_unknown_;

        Graph graph_;
        NodeQueue open_list_;
        MotionModel motion_model_;

    public:
        Astar();
        ~Astar();

        // 清除队列
        void clear_queue();

        inline int get_index(int i, int j)
        {
            return i + j * xs_;
        }
        inline int get_index(const Eigen::Vector2i &pose)
        {
            return pose(0) + pose(1) * xs_;
        }

        // 设置代价地图
        void set_costmap(nav2_costmap_2d::Costmap2D *costmap);
        // 解析栅格的代价值，返回具体的代价值(OCCUPIED、UNKNWON ... )
        float interpret_costmap(int i, int j, nav2_costmap_2d::Costmap2D *costmap);
        // 获取启发代价值
        float get_heuristic_cost(const NodePtr from, const NodePtr &to);
        // 计算从 start 到 end 的路径，并将路径点存储在 path 中，返回路径是否成功创建
        bool create_path(const Eigen::Vector2i &start,const Eigen::Vector2i &end,std::vector<Eigen::Vector2i> &path);
        // 扩张邻节点
        void expansion_neighbors(const int &current_index,std::vector<int> &neighbor_index,std::vector<float> &neighbor_cost);
        // 路径回溯
        bool backtrace_path(NodePtr goal,std::vector<Eigen::Vector2i> &path);
        nav_msgs::msg::OccupancyGrid visualize();
    };
} // namespace astar_planner

#endif // ASTAR_HPP